/**
 * File: robot_control_util.h
 * Brief: 
 * Created by: zhangping
 * Created at: 2017.07.17
 * Modified by:
 * Modified at:
 */

#ifndef ROBOT_CONTROL_UTIL_
#define ROBOT_CONTROL_UTIL_

#include <pthread.h>
#include "communication.h"
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>


namespace robot_node {


    /**
     * @class ControlThread
     * @brief 
     */
    class ControlThread{
      private:
        pthread_t tid_;
        Communication com_;
        ros::NodeHandle &node_;
        
        // 
        static void _PubIns(Communication &com,const geometry_msgs::Twist::ConstPtr& msg);
        
        // Handler for thread.
        static void *_thread_handler(void* this_);
        
        // Clean threads.
        void _clean_threads();

      public:
        // Create thread pool.
        ControlThread(ros::NodeHandle &n, Communication &com);
        pthread_t GetPid();

        // Destroy thread pool.
        ~ControlThread();
    };

}

#endif

